/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * Copyright (c) 2006-2025, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-01-27     RT-Thread    first version
 */

#include <rtthread.h>

#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#include "motor.h"
#include "rgb.h"
#include "uart.h"
#include "camera_rc.h"
#include "servo.h"
#include "pwr.h"
#include "combos.h"
#include "AnoPTv8Par.h"
#include "AnoPTv8Run.h"
#include "encoder.h"
#include "drv_i2c.h"
#include "vehicle.h"
#include "qmi8658.h"

float acc[3],gyro[3];

extern bool isThreadStarted;
uint32_t dt = 0;
void bluetooth_routine(void);
// uint8_t uartBuffer[256];
int main(void)
{
    motor_gpio_init();
    motor_tmr_init();
    at32_tmr_pwm_dma_init();
    drv_soft_i2c_init();
    encoder_task_init();

    rgb_state_set(RGB_STANDBY);
    rt_kprintf("system run...\n");
    uart_init();
    isThreadStarted = true;
    soft_i2c_scan_device();
    qmi8658_init();

    while (1)
    {
        rgb_state_handle();
        bluetooth_routine();
        encoder_routine();
        qmi8658_read_acc_gyro(acc, gyro);
        // rt_kprintf("qmi: %d, %d, %d, %d, %d, %d\n", (int)(acc[0]*1000), (int)(acc[1]*1000), (int)(acc[2]*1000), (int)(gyro[0]), (int)(gyro[1]), (int)(gyro[2]));
        // AnoPTv8TxRunThread1ms();


        uint32_t current_time = rt_tick_get();
        if (current_time > (dt + 1000))
        {
            rt_kprintf("millis: %d\n", current_time);
            dt = current_time;
        }

        vehicle_run();
        rt_thread_mdelay(1);
    }

    return RT_EOK;
}
